%function [x err iter flag convergence msg] = pgs(A, b, x0, max_iter, tol_rel, tol_abs, profile)
function solution = pgs(obj)
% Copyright 2011, Kenny Erleben, DIKU
% modified  to use the standard solver interface by Ying Lu. 
A = obj.dynamics.A_LCP;
b = obj.dynamics.b_LCP;
%x0 = obj.z0_LCP;

N = length(b);
x0 = rand(N, 1);
%max_iter = floor(N/2);
max_iter = 100;
%tol_rel = 0.0001;
%tol_abs = 10 * eps;
profile = false;
U = obj.dynamics.U;
Gf = obj.dynamics.Gf;
NU = obj.dynamics.Vel;
M = obj.dynamics.M;
Gn = obj.dynamics.Gn;
MinvGn = M \Gn;
MinvGf = M \Gf;
h = obj.dynamics.h;
FX = obj.dynamics.Forces;
PSI = obj.contacts.PSI;
nc = length(obj.contacts.PSI);
nd = size(Gf, 2) / nc;
% Just a list of human readable text strings to convert the flag return
% code into something readable by writing msg(flag) onto the screen.
% msg = {'preprocessing';  % flag = 1
%   'iterating';      % flag = 2
%   'relative';       % flag = 3
%   'absolute';       % flag = 4
%   'stagnation';     % flag = 5
%   'local minima';   % flag = 6
%   'nondescent';     % flag = 7solution = struct();
%   'maxlimit'        % flag = 8
%   };

% if nargin < 2
%   error('Too few arguments');
% end
% 
% N    = length(b); % Number of variables
% flag = 1;
% 
% %--- Setup meaningfull default values -------------------------------------
% if nargin<3solution = struct();
%   x0 = zeros(N,1);
% end
% if nargin<4
%   max_iter = floor(N/2);
% end1.0
% if nargin<5
%   tol_rel = 0.0001;
% end
% if nargin<6
%   tol_abs = 10*eps; % Order of 10th of numerical precision seems okay
% end
% if nargin<7
%   profile = true;
% end

%--- Make sure all values are valid ---------------------------------------
max_iter = max(max_iter,1);
% tol_rel  = max(tol_rel,0);
% tol_abs  = max(tol_abs,0);
tol = 1e-8;
x0       = max(0,x0);

%--- Setup values needed while iterating ------------------------------------
convergence = zeros(max_iter,1); % Used when profiling to measure the convergence rate
err         = Inf;               % Current error measure
x           = x0;                % Current iterate
iter        = 1;                 % Iteration counter
flag        = 2;
solution = struct();
solution.total_error = zeros(max_iter, 1);
solution.normal_error = zeros(max_iter, 1); 
solution.friction_error = zeros(max_iter, 1) ;
solution.stick_or_slide = zeros( max_iter, 1);
solution.z = zeros(size(A, 2), max_iter);
solution.iterations = 0;
solution.direction_error = zeros(max_iter, 1);
solution.copositive_normal_error = zeros(max_iter, 1);
solution.copositive_friction_error = zeros(max_iter, 1);
solution.normal_neg_error = zeros(max_iter, 1);
solution.fric_neg_error = zeros(max_iter, 1);

% deal with A for the different properties
% [M1, ~] = lu(A);
% M = M1 + diag(diag(A));
% N = A - M;
A = A + 10^(-6) * eye(size(A, 1));
while iter <= max_iter
  dx = 0;
  for i=1:N
    old_xi = x(i);
    %b(i) = b(i) + N(i, :) * x;
    ri     = b(i) + A(i,:)*x;
    %ri = b(i) + M(i, :) * x;
    Aii    = A(i,i);
    %Aii = M(i, i);
    x(i) = max( 0, old_xi - (ri / Aii) );
    dx = max(dx, abs(x(i) - old_xi));
  end
  
  old_err = err;
    
  % y   = abs( A*x + b );   % Abs is used to fix problem with negative y-values.
  % err = x'*y;
  pn = x(1:nc);
  pf = x(nc+1 : nc+nc*nd);
  s = x(length(x)-nc+1: end);
  NU_ellp1 = NU + MinvGn*pn + MinvGf*pf + M\FX*h;
  solution = updateSolutionData(solution, iter, A, x, b, s, U, pn, PSI, pf, nd, Gf, NU_ellp1);
  
  if profile
    convergence(iter) = err;
  end
  err = x' * (A * x + b);
  
%   
  % Relative stopping criteria
  if (abs(err - old_err) / abs(old_err)) < tol  &&   err < tol    && dx < tol
    break;
  end
  
  % Absolute stopping criteria
%   if err < tol_abs   
%     flag = 4;
%     break;
%   end
%   
%   % Stagnation testing
%   if dx < eps
%     flag = 5;
%     break;
%   end
  
  iter = iter + 1;
end

% if iter>=max_iter
%   flag = 8;
%   iter = iter - 1;
% end

end
